#include "WallFollower.h"
#include "std_msgs/Float64.h"

/**
 * Constructor- sets members as given values and initializes ROS management objects
 */
WallFollower::WallFollower(Driver* driver, double speed, double wallDistance) {

	m_driver = driver;
	m_speed = speed;
	m_goalWallDist = wallDistance;
	m_sideDist = 0;

	m_loop_rate = new ros::Rate(1);
	m_fast_loop_rate = new ros::Rate(100);

	// subscribe to sensors module
	int queue_depth = 1000;

	m_sensors_publisher = m_node_handler.advertise<std_msgs::String>(
			"/Sensors_Module/Subscribers_Channel", 1000);

	// subscribe to front laser
	sub_sensors_fl = m_node_handler.subscribe("/Sensors_Module/Front_Laser",
			queue_depth, &WallFollower::frontLaserHandler, this);

	// subscribe to right part of front laser
	sub_sensors_frl = m_node_handler.subscribe(
			"/Sensors_Module/FrontRight_Laser", queue_depth,
			&WallFollower::frontRightLaserHandler, this);

	// subscribe to left part of front laser
//	sub_sensors_frl = m_node_handler.subscribe(
//			"/Sensors_Module/FrontRight_Laser", queue_depth,
//			&WallFollower::IMPLEMENT_CALLBACK, this);

	// subscribe to right laser
	sub_sensors_rl = m_node_handler.subscribe("/Sensors_Module/Right_Laser",
			queue_depth, &WallFollower::rightLaserHandler, this);
	
	// subscribe to right ultra-sonic
	sub_sensors_right_us = m_node_handler.subscribe("/Sensors_Module/Right_US",
			queue_depth, &WallFollower::rightUSHandler, this);

	// subscribe to left laser
	sub_sensors_ll = m_node_handler.subscribe("/Sensors_Module/Left_Laser",
			queue_depth, &WallFollower::leftLaserHandler, this);
	
	// subscribe to left ultra-sonic
	sub_sensors_left_us = m_node_handler.subscribe("/Sensors_Module/Left_US",
			queue_depth, &WallFollower::leftUSHandler, this);
	
	// subscribe to back laser
	sub_sensors_bl = m_node_handler.subscribe("/Sensors_Module/Back_Laser",
			queue_depth, &WallFollower::backLaserHandler, this);

	// subscribe to back ultra-sonic
	sub_sensors_back_us = m_node_handler.subscribe("/Sensors_Module/Back_US",
			queue_depth, &WallFollower::backUSHandler, this);

	// force connection establishment
	m_loop_rate->sleep();
}

/**
 * Publishes to sensor topic - subscribes to message from
 * sensors when the distance is reached
 */
void WallFollower::subscribeToSensor(sensors_names::ordinal sensor,
		bool shouldSubscribe, double min_distance, double max_distance) {

	// compose message
	char buffer[32];
	snprintf(buffer, sizeof(buffer), "%d%s%d%s%f%s%f", sensor, DELIMITER,
			int(shouldSubscribe), DELIMITER, min_distance, DELIMITER,
			max_distance);
	m_message_to_publish.data = buffer;

	// publish the message to sensors
	m_sensors_publisher.publish(m_message_to_publish);
}

void WallFollower::frontLaserHandler(const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	Event event(sensors_names::FRONT_LASER, atof(msg->data.c_str()));
	event_queue.push(event);
}

/**
 * Create a new event only if front-right sensor does not have
 * an event pending
 */
void WallFollower::frontRightLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	Event front_right_lsr_dummy(sensors_names::FRONT_RIGHT_LASER,0.0);

	int index = event_queue.indexOf(front_right_lsr_dummy);
	if (ERROR_INDEX != index)
	{
		return;
	}

	Event event(sensors_names::FRONT_RIGHT_LASER, atof(msg->data.c_str()));
	event_queue.push(event);
}

void WallFollower::backLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

	ROS_ERROR("IN backLaserHandler");
	// TODO DEBUG!!!
	m_driver->stop();
}

void WallFollower::backUSHandler(const std_msgs::String::ConstPtr& msg) {

	ROS_ERROR("IN backUSHandler");
	// TODO DEBUG!!!
	m_driver->stop();
}

void WallFollower::rightUSHandler(
		const std_msgs::String::ConstPtr& msg) {

	ROS_ERROR("IN WallFollower::rightUSHandler");
	// TODO DEBUG!!!
	m_driver->stop();
}

void WallFollower::rightLaserHandler(const std_msgs::String::ConstPtr& msg) {

	// check validity
	if (NULL == m_driver) {
		ROS_ERROR("DRIVER IS NULL");
		return;
	}

	ROS_ERROR("IN WallFollower::rightLaserHandler");
	// update last sample
	setSideDistance(atof(msg->data.c_str()));
}

void WallFollower::leftUSHandler(const std_msgs::String::ConstPtr& msg) {

	ROS_ERROR("IN leftUSHandler");
	// TODO DEBUG!!!
	m_driver->stop();
}

void WallFollower::leftLaserHandler(
		const std_msgs::String::ConstPtr& msg) {

	ROS_ERROR("IN leftLaserHandler");
	// TODO DEBUG!!!
	m_driver->stop();
}

/**
 * Implements wall following algorithm:
 * finds initial driving direction and maintains parallel driving
 * at indicated distance from wall
 */
void WallFollower::follow() {

	// find initial driving direction
	startDriving();

	// keep driving and maintain direction
	maintainDirection();

	// Stop at exit
	m_driver->stop();
}

void WallFollower::startDriving() {


	// expect data from all sensors
	subscribeToSensor(sensors_names::FRONT_LASER, 		true, -1, 0);
	subscribeToSensor(sensors_names::FRONT_RIGHT_LASER, true, -1, 0);
	subscribeToSensor(sensors_names::RIGHT_LASER, 		true, -1, 0);
	event_queue.setMaxSize(sensors_names::NUM_ITEMS);

	while (event_queue.empty()) {
		ros::spinOnce();
		usleep(100);
	}

	Event closestWall(sensors_names::RIGHT_LASER,MAX_SENSOR_RANGE);

	while (!event_queue.empty()) {
		Event currEvent = event_queue.pop();

		if (currEvent.m_data < closestWall.m_data) {

			closestWall = currEvent;
		}
		// ROS_ERROR("sensor= %d ; data= %f", currEvent.m_sensor, currEvent.m_data);
	}

	ROS_ERROR("closestWall event: sensor= %d ; data= %f", closestWall.m_sensor, closestWall.m_data);

	// got the needed data- cancel subscribe
	subscribeToSensor(sensors_names::FRONT_LASER, 		false, -1, 0);
	subscribeToSensor(sensors_names::FRONT_RIGHT_LASER, false, -1, 0);
	subscribeToSensor(sensors_names::RIGHT_LASER, 		false, -1, 0);
}

bool WallFollower::shouldDriveAway(const Event& dummy) {

	// check if other sensor issued an event
	int index = event_queue.indexOf(dummy);
	if (ERROR_INDEX != index) {

		// check if fellow sensor is close to the wall too
		Event fellowSensor = event_queue.getElement(index);
		if (fellowSensor.m_data <= m_goalWallDist - TOLERANCE) {

			// fellow's event is not needed
			event_queue.erase(index);

			// turn away
			return true;

		} else { // event in queue can be either sensor is too close or too far

			if (dummy.m_sensor == sensors_names::FRONT_RIGHT_LASER
					|| dummy.m_sensor == sensors_names::FRONT_LEFT_LASER) {

				// front end is too far from the wall, cannot drive away
				return false;

			} else {

				// side sensor is too far- must be ignored
				event_queue.erase(index);

				// turn away
				return true;
			}

		}
	}

	// other event does not exist, turn away
	return true;
}

// call only when a sensor is too far from the wall
bool WallFollower::shouldDriveCloser(const Event& dummy) {

	// check if other sensor did not issue an event, turn away
	if (!isSensorPending(dummy)) {
		return true;
	}

	// check if fellow sensor is far to the wall too
	int fellowIndex = event_queue.indexOf(dummy);
	Event fellowSensor = event_queue.getElement(fellowIndex);
	if (isFar(fellowSensor)) {

		// fellow's event is not needed
		event_queue.erase(fellowIndex);

		// turn away
		return true;

	} else { // event in queue can be either sensor is too close or too far

		if (isSensorFront(dummy)) {

			// front end is too far from the wall, cannot drive away
			return false;

		} else {

			// side sensor is too far- must be ignored
			event_queue.erase(fellowIndex);

			// turn away
			return true;
		}

	}
}

/**
 * Maintain robot's correct driving direction:
 * If robot's distance exceeds the range bounded by the error tolerant,
 * fix its driving direction
 */
void WallFollower::maintainDirection() {

	bool not_alligned = true;
	// set maximal numbers of events to handle
	event_queue.setMaxSize(MSG_QUEUE_SIZE);

	// activate necessary sensors
	subscribeToSensor(sensors_names::FRONT_LASER, 		true, CORNER_DISTANCE, MAX_SENSOR_RANGE);
	subscribeToSensor(sensors_names::FRONT_RIGHT_LASER, true, m_goalWallDist-TOLERANCE, m_goalWallDist+TOLERANCE);
	subscribeToSensor(sensors_names::RIGHT_LASER, 		true, MAX_SENSOR_RANGE, 0);

	// set for how long should handle each event
	long int timeForEvent = TIME_FOR_EVENT; // calculation of time: 0.25 = 25e-2 = (25e4)*1e-6

	// read events from queue and handle them
	while (ros::ok()) {

		ros::spinOnce();

		while (!event_queue.empty()) {

			// handle event at queue's head
			Event currEvent = event_queue.pop();

			switch (currEvent.m_sensor) {

			// handle obstacle ahead
			case  sensors_names::FRONT_LASER: {

				// handle obstacles
				if (currEvent.m_data <= OBSTACLE_DISTANCE) {

					// if too close, stop
					if (currEvent.m_data <= EMERGENCY_DISTANCE) {
						m_driver->stop();
						break;
					}

					// not too close to obstacle, stop and try bypassing
					m_driver->stop();
					usleep(50000);
					m_driver->turn(ESCAPE_CORNER_ANGLE);
					usleep(50000);

				} else {
					// obstacle is a corner- turn left
					m_driver->turn(ESCAPE_CORNER_ANGLE);
				}

				break;
			}

			// robot is either too close or too far from the wall
			case sensors_names::FRONT_RIGHT_LASER: {

				// TODO
				ROS_INFO("FRONT_RIGHT_LASER--currEvent.m_data: %.3f",currEvent.m_data);

				// fix robot's distance from wall according to sampled data
				fixDistance(currEvent);
				break;
			}

			case sensors_names::RIGHT_LASER: {
				// event is handled by storing newest sample
				break;
			}


				default: {
					ROS_ERROR("WallFollower switch default unpredictable call! value= %d", currEvent.m_sensor);
				}
			}	//Switch end

			// set process to sleep in order to enable influence of event handling
			usleep(timeForEvent);
		}

		// no event, keep driving forward
		m_driver->move(m_speed, 0);
	}
}

/**
 * Method is called if an event has been created- robot is either too close
 * or too far from the wall.
 * Uses sampled distance from wall to decide whether should turn towards the
 * wall or away from it:
 * calculates how much should turn in order to be parallel and how much should
 * turn in order to keep right distance.
 * If angles are at different direction, turns in vector's sum.
 * If both are of the same direction, turns in the maximal angle among the two.
 * After turning angle is calculated, it is scaled up because robot's architecture
 * turns in radians per second, but event should be handled in TIME_FOR_EVENT microseconds
 */
void WallFollower::fixDistance(const Event& currEvent) {

	// calculate angle to align robot
	double align_angle = align(currEvent);

	// calculate angle to fix its distance
	// (order of operands must be as written, otherwise turn angle's sign is wrong)
	double fix_angle = turnAngle(m_goalWallDist - currEvent.m_data);

	// scale-up angles
	double scale = 100000/TIME_FOR_EVENT;

	// TODO
	ROS_INFO("fix angle: %f, rads: %f", fix_angle*(180/(2*HALF_PI)), fix_angle);

	// choose turning angle

	if ((fix_angle*align_angle) <= 0) {
		m_driver->turn(scale*(fix_angle + align_angle));
		return;
	}

	if (fix_angle < align_angle) {
		m_driver->turn(scale*align_angle);
		return;
	}

	m_driver->turn(scale*fix_angle);
}

/**
 * Calculates how much should turn (in radians) in order to fix
 * robot's orientation, so it will be parallel to the wall.
 * Calculated value does not guarantee that robot will be at correct
 * distance from wall.
 * See manual for the explained calculation
 */
double WallFollower::align(const Event& event) {

	// read last sample from side sensor
	double sideDist = getSideDistance();

	// calculate angle to fix tilt (negative angle means turn right)
	double fixAngle = -atan((event.m_data-sideDist) / FRONT_SIDE_DIST);

	// TODO
	ROS_INFO("alignment angle: %f, rads: %f. Side Distance = %f", fixAngle*(180/(2*HALF_PI)), fixAngle,sideDist);

	// return result
	return fixAngle;
}

/**
 * Calculates how much should turn (in radians) in order to fix
 * robot's distance from wall- turn towards the wall or away from the wall.
 * Calculated value does not guarantee that robot will be parallel to the wall.
 * See manual for the explained calculation
 */
double WallFollower::turnAngle(double horizontalDist) {

	// avoid zero division
	if (0 == horizontalDist) {
		return 0; // do not turn
	}

	// TODO DEBUG PRINT!!!
	ROS_INFO("horizontal distance: %f ; turn angle (rads): %f", horizontalDist, atan(horizontalDist / (FIXING_DISTANCE*(m_speed+1))));
	//

	// calculate angle
	return atan(horizontalDist / (FIXING_DISTANCE*(m_speed+1)));
}

/**
 * Get newest distance sampled from side sensor.
 * Access is synchronized
 */
double WallFollower::getSideDistance() {

	double sideDist = 0;
	std::unique_lock<std::mutex>
	mlock(m_mutex);
	sideDist = m_sideDist;
	mlock.unlock();
	return sideDist;
}

/**
 * Set newest distance sampled from side sensor.
 * Access is synchronized
 */
void WallFollower::setSideDistance(double newDist) {

	std::unique_lock<std::mutex>
	mlock(m_mutex);
	m_sideDist = newDist;
	ROS_ERROR("new side distance: %f", m_sideDist);
	mlock.unlock();
}

double WallFollower::boostAngle(double fixAngle) {

	double absVal = abs(fixAngle);
	double sign = fixAngle / absVal;
	return (absVal < MIN_ANGLE) ? (sign * MIN_ANGLE) : fixAngle + (sign * MIN_ANGLE);
}

bool WallFollower::isClose(const Event& event) {
	return event.m_data <= (m_goalWallDist-TOLERANCE);
}

bool WallFollower::isFar(const Event& event){
	return event.m_data >= (m_goalWallDist + TOLERANCE);
}

bool WallFollower::isSensorPending(const Event& event) {
	return (event_queue.indexOf(event) != ERROR_INDEX);
}

bool WallFollower::isSensorFront(const Event& event) {
	return (event.m_sensor == sensors_names::FRONT_RIGHT_LASER)
			|| (event.m_sensor == sensors_names::FRONT_LEFT_LASER);
}
